#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S3,     HTAC,                sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
<<<<<<< .mine
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*/
//#define a mtr_S1_C1_1
//#define b mtr_S1_C1_2
//#define c mtr_S1_C2_1
//#define d mtr_S1_C2_2

#ifndef FBRIDGE
#define FBRIDGE
#endif

/*********************************************************************\
*																																	  	*
* PROGRAM: Over Bridge Driver   	      														  *
* VERSION: 1.4																										    *
* PURPOSE: This program is used to get an omni diretional robot       *
*          over the bridge, since the robot doesn't drive straight    *
* AUTHOR: Aaron Jaeger                                                *
* DATE:		 April 2011  	  																					  *g
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/

/**
*
* This function lets the NXT know which IR sensor is Falling of the Bridge
* Value of fallingMap changes if the IR senses a drop, or a the surface getting closer
* Each sensor is represented in binary below
* 0001 FL
* 0010 FR
* 0100 BL
* 1000 BR
*
*/


/**
* fallingLeft - True when:
*  0001 front left IR is falling
*  0100 Back left IR is Falling
*  0101 Front Left & Back left IR are Falling
*
* fallingRight - True when:
*  0010 Front Right IR is falling
*  1000 Back Right IR is Falling
*  1010 Front Right & Back Right are Falling
*
* allOn - True when:
*   no sides are falling
*  0000 <-- Falling map
*
*/
int fallingMap = 0;

bool fallingLeft() {
  return (fallingMap ==IR_FL)
  || (fallingMap == IR_BL)
  || (fallingMap == (IR_FL + IR_BL));
}

bool fallingRight () {
  return (fallingMap == (IR_FR + IR_BR)) || (fallingMap == IR_FR) || (fallingMap == IR_BR);
}

bool allOn () {
  return fallingMap == 0;
}

/*
* Below are the two debuging Functions
* showValues - displays all the IR values
* debugFalling - Displays which sides the robot thinks is falling off.
*/
void showValues() {
  updateFallingMap();
  nxtDisplayTextLine(1,"FR %d",_IRA);
  nxtDisplayTextLine(2,"BR %d",_IRB);
  nxtDisplayTextLine(3,"BL %d",_IRC);
  nxtDisplayTextLine(4,"FL %d",_IRD);

  writeDebugStreamLine("");

}

void debugFalling() {
  if(fallingLeft() == true) {
    nxtDisplayTextLine(2,"falling left");
    } else {
    nxtDisplayTextLine(2,"------------");
  }
  if(fallingRight() == true) {
    nxtDisplayTextLine(3,"falling right");
    } else {
    nxtDisplayTextLine(3,"------------");
  }

  if(allOn() == true) {
    nxtDisplayTextLine(7,"allOn");
    } else {
    nxtDisplayTextLine(7,"------------");
  }
}

int _Y_axis = 0;
int _Y_axisCntrl;
bool offBridge () {
  _Y_axis = AccelReadingY();
  //writeDebugStreamLine("Reading: %4d", _Y_axis);
  return _isBetween(_Y_axis, _Y_axisCntrl , 5);

}
void encoderClean () {
  nMotorEncoder[d] = 0;
  nMotorEncoder[a] = 0;
}

void correctDrive(bool df, int encodeCount, int spd, int encodFix) {
  while (abs (nMotorEncoder[d]) < encodeCount) {
  int dspeed = (df) ? spd : -spd;

    bool dirc = false;  //false -> right, true -> left
    eraseDisplay();
    fallingMap = updateFallingMap();
    writeDebugStreamLine("RE-STARTED-LOOP");
    PlaySound(soundShortBlip );
    /*******************************************/
    forward(dspeed);
    writeDebugStreamLine("encoder b %4d", nMotorEncoder[b]);
    while(allOn()) {

      fallingMap = updateFallingMap();
      PlaySound(soundShortBlip );
      if (abs (nMotorEncoder[d]) > encodeCount) {
        return;
      }
    }
    writeDebugStreamLine("FALLING?");
    if (fallingLeft()) {
    dirc = (df) ? true : false;
    }
    if (fallingRight()) {
    dirc = (df) ? false : true;
    }
    /**************************************/
    if (dirc) {
      nMotorEncoder[a] = 0;
      frontr(dspeed);
      writeDebugStreamLine("FIXING ( TRUE");
      while((nMotorEncoder[c] < encodFix) && (nMotorEncoder[c] > -encodFix)) {
        //writeDebugStreamLine("encC: %4d", nMotorEncoder[c]);
        if (abs(nMotorEncoder[b]) > encodeCount) {
          return;
        }
      }
    }
    else {
      nMotorEncoder[a] = 0;
      frontl(dspeed);
      writeDebugStreamLine("FIXING ( FALSE");
      while(abs(nMotorEncoder[c]) < encodFix) {
        //writeDebugStreamLine("EncC: %4d", nMotorEncoder[c]);
        if (abs(nMotorEncoder[b]) > encodeCount) {
          return;
        }
      }
      //writeDebugStreamLine("NOT STUCK")
    }///if dirc
    /**************************************/
  }// (while encoder[a] < ...) at this point, if we went strait we should be off the bridge.
  /****************************************/

}

///////////////////////////////////////////////////////////////////////////////////////////////
//  Following function is the Bridge crosser
//
//
//
///////////////////////////////////////////////////////////////////////////////////////////////
void crossBridge(bool dforward, int _encodeVal, int speed, bool crossover) {
  initIRLowPass();
  if(crossover) {
    calibrateAccel();
    _Y_axisCntrl = AccelReadingY();
  }
  int sspeed = (dforward) ? SLOW_SPD : -SLOW_SPD;
  encoderClean();
  correctDrive(dforward,_encodeVal,speed,R_FIX);
  servo[frontarm] = SERVO_FRONTARM_UP;
  /****************************************/
  stopmot();
  if (crossover) {
    wait1Msec(400);
    while(!offBridge()) {
      writeDebugStreamLine("CHECKING IF OFF");

      //displayAccel();
      stopmot();
      wait1Msec(370);
      nMotorEncoder[b] = 0;
      correctDrive(dforward, E_CROSS_TRY_AGAIN, sspeed, TRY_AGAIN_R_FIX);
    }
    stopmot();
    writeDebugStreamLine("I'M OFF!");
  }
  /**************************************/
}
